//主节点
//功能介绍：
//------------1、等待地面站的系统开启指令，如果系统开启，则可执行功能2-9
//------------2、通过回调函数，获取无人机当前的GPS位置信息
//------------3、从串口接收信息，并进行解析&分类存储
//------------4、如果采用时间溢出判断，则面临无法收全的问题，此时就要开启：用上次接收的信息去补足此次信息
//------------5、信息的打包，包括：所有无人机的位置&所有飞机的起飞指令（从上层节点中订阅此话题）
//------------6、信息的发送，如需多次发送，则可计时or记次（发一次应该就行）
//------------7、更新【上一次信息】
//------------8、整合&发布 所有无人机的位置 话题信息
//------------9、清空相关参数
#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/String.h> 
#include <std_msgs/Empty.h> 
#include <std_msgs/Bool.h>
#include <std_msgs/UInt8.h>
#include <geometry_msgs/PoseStamped.h>
#include "serialport/ALL_UAVs_LLA.h"
// #include "geographic_msgs/GeoPoint.h"
#include "sensor_msgs/NavSatFix.h"
#include <wrzf_pkg/DroneState.h>
#include <string>
#include <time.h>
#include <sstream>
#include "serialport/data_make.h"

using namespace std;
serial::Serial ser; //声明串口对象 
const int Blen_child = 17;    //子节点的数据长度
const int Blen_host  = 54;    //主节点的数据长度
const int Blen_stop  = 9;     //子节点信息发送许可证---信息
const int Blen_start  = 6;     //子节点信息发送许可证---信息
uint8_t recv_buff[140];
uint8_t send_buff[54];
uint8_t stop_buff[9];
int len_send, lend_recv;
int uav_ID, send_stop;
std_msgs::Bool takeoff_flag;                // 【发布】来自地面站的系统启动指令
uint8_t takeoff_order_recv;      // 【订阅】无人机的起飞顺序指令
uint8_t takeoff_order_send;      // 【发送】无人机的起飞顺序指令

struct uav_MSG
{
    double longitude;
    double latitude;
    double altitude;
};
uav_MSG uav={},uav4={},          uav1={},          uav2={},          uav3={};
uav_MSG               uav4_last={},uav1_last={},uav2_last={},uav3_last={};

// 【回调函数】无人机当前位置
wrzf_pkg::DroneState uav_current_state;
void uav_lla_cb(const wrzf_pkg::DroneState::ConstPtr& msg)
{
    uav_current_state = *msg;
}
// 【回调函数】无人机起飞顺序指令
void takeoff_order_cb(const std_msgs::UInt8::ConstPtr& msg)
{
    takeoff_order_recv = msg->data;
}
int base_fly_command=1;
// int fly_command=7;
// int fly_command_num=0;
// 参数********************************************************************************************
const int frequence  = 30;    
const int ttimeout  = 7;    
const int readnum  = 33;     
// 主函数********************************************************************************************
int main (int argc, char** argv) 
{ 
    ros::init(argc, argv, "host_node2");    //初始化节点 
    ros::NodeHandle nh;                                 //声明节点句柄 
    ros::Rate loop_rate(frequence);                            //设置循环频率，括号内参数即为频率
    cout << "start host_node2" << endl;

    //【订阅】无人机的自身位置
    ros::Subscriber uav_lla_sub = nh.subscribe<wrzf_pkg::DroneState>("/px4_command/drone_state", 10,uav_lla_cb);
    //【订阅】无人机的起飞顺序指令
    ros::Subscriber takeoff_order_sub = nh.subscribe<std_msgs::UInt8>("/takeoff_order", 10,takeoff_order_cb);
    //【发布】所有无人机的位置信息
    ros::Publisher uavs_lla_pub = nh.advertise<serialport::ALL_UAVs_LLA>("/all_uavs_position/global", 10);
    //【发布】系统启动指令
    ros::Publisher takeoff_flag_pub = nh.advertise<std_msgs::Bool>("/px4_command/takeoff_flag", 10);

    // 发布无人机的位置信息的准备
    serialport::ALL_UAVs_LLA uavs_lla_arry;
    geographic_msgs::GeoPoint geo_msg[4];   //由飞机数量固定长度
    std::vector< geographic_msgs::GeoPoint> geo_arry;

    // 串口设置***************************************************************************************
    try 
    { 
        //设置串口属性，并打开串口 
        ser.setPort("/dev/ttyUSB0"); 
        ser.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(ttimeout); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 

    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 
    //检测串口是否已经打开，并给出提示信息 
    if(ser.isOpen()) 
    { 
        ROS_INFO_STREAM("Serial Port initialized"); 
    } 
    else 
    { 
        return -1; 
    }
    // ************************************************************************************************
    // 定义数据统计变量
    struct SINFO
    {
        int send_num;
        int send_r_num;
        float send_r_rate;
        int recv_num;
        int recv_r_num;
        float recv_r_rate;
        int fail_num;
        int uav_recv_all;
    };
    SINFO s_info = {};

    //消息的定义
    //2号消息：子节点能否发送信息的许可
    stop_buff[0] = 0xa4;
    stop_buff[1] = 0xf1;
    stop_buff[2] = 0x2;
    stop_buff[3] = 0x1;     // 现在是4架飞机，所以只用低4位。全为1代表都可发送
    stop_buff[4] = 0x1; 
    stop_buff[5] = 0x1; 
    stop_buff[6] = 0x1; 
    stop_buff[7] = data_crc(stop_buff,Blen_stop-2);
    stop_buff[8] = 0x4a;
    
    //计时相关参数
    bool timeout = 0, loop_state, send_state =1,recv_state = 0;
    clock_t start_time=0,end_time=0;
    int time_dif=0;
    // 正式开始工作
    while(ros::ok()) 
    {
        cout << endl;
        //执行回调函数
        ros::spinOnce();
        // 系统开启指令**********************************************************************
        // while(!takeoff_flag.data)
        // {
        //     lend_recv = ser.read(recv_buff,140); // 接收系统启动指令
        //     int i = 0;
        //     while (i < lend_recv)
        //     {
        //         if (recv_buff[i] == 0xa4 && recv_buff[i+Blen_start-1] == 0x4a)
        //         {
        //             if (recv_buff[i+1] == 0xc0 && recv_buff[i+2] == 0x0 && (lend_recv-i) >= Blen_start)
        //             {
        //                 if (data_crc(&recv_buff[i],Blen_start-2) == recv_buff[i+Blen_start-2])
        //                 {
        //                     if (recv_buff[i+3] == 1)
        //                         {takeoff_order_send = 1; }
        //                     else
        //                         {takeoff_order_send = 0; }
        //                     // takeoff_flag.data = (bool)recv_buff[i+3];
        //                     // 将系统启动指令发布出去
        //                     //takeoff_flag_pub.publish(takeoff_flag);
        //                     break;
        //                 }
        //                 else
        //                     i = i + Blen_start;
        //             }
        //             else
        //                 i++;
        //         }
        //         else
        //             i++;
        //     }
        //     memset(recv_buff,0,sizeof(recv_buff));
        //     // ros::spinOnce();
        //     loop_rate.sleep();
        // }

        //更新自身信息****************************************************
        // uav4.longitude = 144.131513565;
        // uav4.latitude = 44.2656438;
        // uav4.altitude = 4.25451348;
        // uav3.longitude = 133.2135413;
        // uav3.latitude = 33.2165385;
        // uav3.altitude = 3.12156348;
        //cout << "_send_takeoff_" <<endl;
        uav4.longitude    = abs(uav_current_state.longitude);
        uav4.latitude     = abs(uav_current_state.latitude);
        uav4.altitude     = abs(uav_current_state.position[2]);
        uav_ID |=0x1000;
        //接收信息****************************************************
        //进入接收循环前的时间标记
        start_time = clock();
        timeout = 0;
        //s_info.recv_num++;
        int again_flag1=0;
        while(uav_ID != 0x1111)// && ! timeout)
        {
            ////////////////////////////////////////////////////////////////////////////////////////////////tmp
            if(base_fly_command==1)
            {
                lend_recv = ser.read(recv_buff,140); // 接收系统启动指令
                int i = 0;
                while (i < lend_recv)
                {
                    if (recv_buff[i] == 0xa4 && recv_buff[i+Blen_start-1] == 0x4a)
                    {
                        if (recv_buff[i+1] == 0xc0 && recv_buff[i+2] == 0x0 && (lend_recv-i) >= Blen_start)
                        {
                            if (data_crc(&recv_buff[i],Blen_start-2) == recv_buff[i+Blen_start-2])
                            {
				                // fly_command=recv_buff[i+3];
				                // fly_command_num++;

                                if (recv_buff[i+3] == 1)
                                    {takeoff_order_send = 1; base_fly_command=0;}
                                else
                                    {takeoff_order_send = 0; }
                                // takeoff_flag.data = (bool)recv_buff[i+3];
                                // 将系统启动指令发布出去
                                //takeoff_flag_pub.publish(takeoff_flag);
                                break;
                            }
                            else
                                i = i + Blen_start;
                        }
                        else
                            i++;
                    }
                    else
                        i++;
                }
                memset(recv_buff,0,sizeof(recv_buff));
            }
            




            ///////////////////////////////////////////////////////////////////////////////////////////////tmp

             int again_flag=0;
            //单次接收
            // lend_recv = ser.read(recv_buff,ser.available()); 
            s_info.recv_num++;
            lend_recv = ser.read(recv_buff,readnum); // 接收字节数不同，会影响串口效率
            if (lend_recv) 
            {
               // s_info.recv_num++;
              //  cout << "base_recv:" << lend_recv <<endl;
            }
            //消息的解析
            int i = 0;
            while(i< lend_recv)
            {
                if (recv_buff[i] == 0xa4 && recv_buff[i+Blen_child-1] == 0x4a)
                {
                    // cout << "recv_start_end:" << endl;
                    //  接收消息1（如果飞机多那就改成 recv_buff[i+1] & 0xc0）
                    if ((recv_buff[i+1] & 0xc0) && recv_buff[i+2] == 0x01 && (lend_recv-i) >= Blen_child)
                    {
                        // 校验位检验
                        s_info.recv_r_num++;
                        if (data_crc(&recv_buff[i],Blen_child-2) == recv_buff[i+Blen_child-2])
                        {
                           //  cout << "crc_start:" << endl;
                            recv_state = 1;
                            uav.longitude = merge(recv_buff[i+3],recv_buff[i+4],recv_buff[i+5],recv_buff[i+6]);
                            uav.latitude =     merge(recv_buff[i+7],recv_buff[i+8],recv_buff[i+9],recv_buff[i+10]);
                            uav.altitude =     merge(recv_buff[i+11],recv_buff[i+12],recv_buff[i+13],recv_buff[i+14]);
                            int uav_num = recv_buff[i+1] - 0xc0;
                            switch (uav_num)
                            {
                                case 1:{uav1 = uav;uav_ID |=0x1; again_flag++;} break;
                                case 2:{uav2 = uav;uav_ID |=0x10; again_flag++;} break;
                                case 3:{uav3 = uav;uav_ID |=0x100; again_flag++;} break;
                            }
                            if( again_flag>1)
                            {
                                 again_flag1=again_flag;

                            }
                            cout << "收到px_" << uav_num << " msg:" << endl;
                            //cout << "uav:" << uav.longitude  << "  " << uav.latitude  << "  " << uav.altitude  <<endl;
                            if(i+16<=lend_recv)
                            {
                                i = i + Blen_child;
                            }
                            else
                            {
                                break;
                            }
                           // break;//这句话导致只能接收一次
                        }
                        else
                        {
                            recv_state = 0; 
                            s_info.fail_num++;
                            i = i + Blen_child;
                           //cout << "crc_fail" << endl;
                        }
                    }
                    else
                        i++;
                     //接收消息2
                }
                else
                    i++;//检测包头
            }
           
        }
        memset(recv_buff,0,sizeof(recv_buff));
        //判断信息接收情况*********************************************************************************
        if (uav_ID == 0x1111)
            s_info.uav_recv_all++;
        else if (uav_ID != 0x1111)
        {
            //用上次的发送信息进行信息补位
            cout << "进行信息补位" << endl;
            if (!(uav_ID & 0x1))
                 uav1 = uav1_last;
            if (!(uav_ID & 0x10))
                 uav2 = uav2_last; 
            if (!(uav_ID & 0x100))
                 uav3 = uav3_last; 
        }
        //信息打包**************************************************************************
        {
            send_buff[0] = 0xa4;
            send_buff[1] = 0xf1;
            send_buff[2] = 0x1;
            send_buff[3] = split(uav1.longitude,1); send_buff[4] = split(uav1.longitude,2); send_buff[5] = split(uav1.longitude,3); send_buff[6] = split(uav1.longitude,4);
            send_buff[7] = split(uav1.latitude,1);     send_buff[8] = split(uav1.latitude,2);     send_buff[9] = split(uav1.latitude,3);     send_buff[10] = split(uav1.latitude,4);
            send_buff[11] = split(uav1.altitude,1);     send_buff[12] = split(uav1.altitude,2);     send_buff[13] = split(uav1.altitude,3);     send_buff[14] = split(uav1.altitude,4);
            send_buff[15] = split(uav2.longitude,1); send_buff[16] = split(uav2.longitude,2); send_buff[17] = split(uav2.longitude,3); send_buff[18] = split(uav2.longitude,4);
            send_buff[19] = split(uav2.latitude,1);     send_buff[20] = split(uav2.latitude,2);     send_buff[21] = split(uav2.latitude,3);     send_buff[22] = split(uav2.latitude,4);
            send_buff[23] = split(uav2.altitude,1);     send_buff[24] = split(uav2.altitude,2);     send_buff[25] = split(uav2.altitude,3);     send_buff[26] = split(uav2.altitude,4);
            send_buff[27] = split(uav3.longitude,1); send_buff[28] = split(uav3.longitude,2); send_buff[29] = split(uav3.longitude,3); send_buff[30] = split(uav3.longitude,4);
            send_buff[31] = split(uav3.latitude,1);     send_buff[32] = split(uav3.latitude,2);     send_buff[33] = split(uav3.latitude,3);     send_buff[34] = split(uav3.latitude,4);
            send_buff[35] = split(uav3.altitude,1);     send_buff[36] = split(uav3.altitude,2);     send_buff[37] = split(uav3.altitude,3);     send_buff[38] = split(uav3.altitude,4);
            send_buff[39] = split(uav4.longitude,1); send_buff[40] = split(uav4.longitude,2); send_buff[41] = split(uav4.longitude,3); send_buff[42] = split(uav4.longitude,4);
            send_buff[43] = split(uav4.latitude,1);     send_buff[44] = split(uav4.latitude,2);     send_buff[45] = split(uav4.latitude,3);     send_buff[46] = split(uav4.latitude,4);
            send_buff[47] = split(uav4.altitude,1);     send_buff[48] = split(uav4.altitude,2);     send_buff[49] = split(uav4.altitude,3);     send_buff[50] = split(uav4.altitude,4);
            send_buff[51] = (takeoff_order_recv | takeoff_order_send);   //起飞标志位
            send_buff[52] = data_crc(send_buff,Blen_host-2);
            send_buff[53] = 0x4a;
        }
        //发送信息***************************************************************************
        //重新计时
        start_time = clock();
        timeout = 0;
        //发送信息
        // while (!timeout)
        // {

            len_send = ser.write(send_buff,Blen_host); 
            s_info.send_num++;
            // ros::Duration(0.05).sleep(); //sleep for 50 ms
            if (len_send == Blen_host)
            {
                // cout << "send successful!" << endl;
                s_info.send_r_num++;
            }
        //上次信息更新********************************************************************************
        uav1_last = uav1; uav2_last = uav2; uav3_last = uav3; uav4_last = uav4; 
        // cout << "uav1:" << uav1.longitude  << "  " << uav1.latitude  << "  " << uav1.altitude  <<endl;
        // cout << "uav2:" << uav2.longitude  << "  " << uav2.latitude  << "  " << uav2.altitude  <<endl;
        // cout << "uav3:" << uav3.longitude  << "  " << uav3.latitude  << "  " << uav3.altitude  <<endl;
        // cout << "uav4:" << uav4.longitude  << "  " << uav4.latitude  << "  " << uav4.altitude  <<endl;

        // 判断系统启动指令并发布出去
        if (takeoff_order_recv & 0x0F)//1111
            takeoff_flag.data = 1;
        else
            takeoff_flag.data = 0;
        takeoff_flag_pub.publish(takeoff_flag);
        //发布话题信息********************************************************************************
        geo_msg[0].longitude = uav1.longitude;
        geo_msg[0].latitude     = uav1.latitude;
        geo_msg[0].altitude     = uav1.altitude;
        geo_msg[1].longitude = uav2.longitude;
        geo_msg[1].latitude     = uav2.latitude;
        geo_msg[1].altitude     = uav2.altitude;
        geo_msg[2].longitude = uav3.longitude;
        geo_msg[2].latitude     = uav3.latitude;
        geo_msg[2].altitude     = uav3.altitude;
        geo_msg[3].longitude = uav4.longitude;
        geo_msg[3].latitude     = uav4.latitude;
        geo_msg[3].altitude     = uav4.altitude;
        geo_arry.push_back(geo_msg[0]);
        geo_arry.push_back(geo_msg[1]);
        geo_arry.push_back(geo_msg[2]);
        geo_arry.push_back(geo_msg[3]);
        uavs_lla_arry.all_uavs_lla = geo_arry;
        uavs_lla_pub.publish(uavs_lla_arry);
	    geo_arry.clear();
        // ros::spinOnce();
        //事后工作*************************************************************************************
        uav_ID = 0;
        memset(send_buff,0,sizeof(send_buff));
        //uav4 = {}; uav1 = {}; uav2 = {}; uav3 = {}; uav = {};

        // loop_state = loop_rate.sleep();
        // cout << "loop_state is " << loop_state << endl;
        // cout << "send_num: " << s_info.send_num << "\t" << "send_r_num: " << s_info.send_r_num << endl;
       // cout << "crcfail_num:" << s_info.fail_num << endl;
       loop_state = loop_rate.sleep();
        cout << "uav_recv_all:" << s_info.uav_recv_all << endl;
        cout <<"start_end_num: " << s_info.recv_r_num << endl;
        cout << "recv_num: " << s_info.recv_num<< endl;
        cout << "crcfail_num:" << s_info.fail_num<< endl;
        cout << "again_flag1:" <<again_flag1<< endl;
        // cout << "fly_command7:" <<fly_command<< endl;
        // cout << "fly_command_num:" <<fly_command_num<< endl;
        // cout <<  endl;
    }
    return 0;
}
